#include<ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <pcl_ros/point_cloud.h>
#include <sensor_msgs/Imu.h>
#include <mutex>
#include <tf/transform_broadcaster.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/passthrough.h>
#include <pcl/io/pcd_io.h> //PCL的PCD格式文件的输入输出头文件

#include <tf2_ros/transform_listener.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/buffer.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

std::mutex my_mutex_;
bool odom_received_=false,is_first_get_=false;
sensor_msgs::Imu imu_q_cur_;
nav_msgs::Odometry current_odom_,pose_show_;
std::vector<nav_msgs::Odometry> current_odom_vec_,last_odom_vec_;

sensor_msgs::PointCloud2 pc_ros_msg_;

ros::Publisher odom_pose_pub_,odom_path_pub_,cloud_pub_;
nav_msgs::Path odom_path_;

int pc_num_=1;



//motion calibrate
using myPointType=pcl::PointXYZ;
double pc_time_last_=0,pc_time_current_=0;
pcl::PointCloud<pcl::PointXYZ>::Ptr pc_frame_,pc_order_,pc_final_;
double pc_startOrientation_,pc_endOrientation_;

int N_SCAN_ = 16;
int Horizon_SCAN_ = 1800;
float ang_res_x_ = 0.2;
float ang_res_y_ = 2.0;
float ang_bottom_ = 15.0+0.1;
float time_scan_gap_=0.1/Horizon_SCAN_;

Eigen::Vector3d odom_is_far_cur_,odom_is_far_last_(0,0,0);

std::mutex odom_vec_mutex_;
double odom_x_bad=654321;

template <typename T>
void clearVector(std::vector<T>& vt ){
    std::vector<T> vcTemp;
    vcTemp.swap(vt);
}

/*void findStartEndAngle(){
    // start and end orientation of this cloud
    pc_startOrientation_ = -atan2(pc_frame_->points[0].y, pc_frame_->points[0].x);
    pc_endOrientation_  = -atan2(pc_frame_->points[pc_frame_->points.size() - 1].y,
                                     pc_frame_->points[pc_frame_->points.size() - 1].x) + 2 * M_PI;
    if (pc_endOrientation_ - pc_startOrientation_ > 3 * M_PI) {
        pc_endOrientation_ -= 2 * M_PI;
    } else if (pc_endOrientation_ - pc_startOrientation_ < M_PI)
        pc_endOrientation_ += 2 * M_PI;
}*/

void projectPointCloud(){
    // range image projection
    float verticalAngle, horizonAngle, range;
    size_t rowIdn, columnIdn, index, cloudSize;
    myPointType thisPoint;

    pc_order_.reset(new pcl::PointCloud<myPointType>);
    pc_order_->points.resize(N_SCAN_*Horizon_SCAN_);
    myPointType badPoint;
    badPoint.x=std::numeric_limits<double>::quiet_NaN();
    std::fill(pc_order_->points.begin(), pc_order_->points.end(), badPoint);

    cloudSize = pc_frame_->points.size();
    for (size_t i = 0; i < cloudSize; ++i){

        thisPoint.x = pc_frame_->points[i].x;
        thisPoint.y = pc_frame_->points[i].y;
        thisPoint.z = pc_frame_->points[i].z;
        // find the row and column index in the iamge for this point

        verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
        rowIdn = (verticalAngle + ang_bottom_) / ang_res_y_;

        if (rowIdn < 0 || rowIdn >= N_SCAN_)
            continue;

        horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;

        columnIdn = -round((horizonAngle-90.0)/ang_res_x_) + Horizon_SCAN_/2;
        if (columnIdn >= Horizon_SCAN_)
            columnIdn -= Horizon_SCAN_;

        if (columnIdn < 0 || columnIdn >= Horizon_SCAN_)
            continue;

        range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
        if (range < 1)
            continue;

        index = columnIdn  + rowIdn * Horizon_SCAN_;
        pc_order_->points[index] = thisPoint;
    }
}

void getPointCloudAndProject(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){

    std::unique_lock<std::mutex> lock_odom(odom_vec_mutex_);
    pc_time_current_ = laserCloudMsg->header.stamp.toSec();

    last_odom_vec_=current_odom_vec_;
    clearVector(current_odom_vec_);
    if(last_odom_vec_.size()>3){
        current_odom_vec_.push_back(last_odom_vec_[last_odom_vec_.size()-3]);
        current_odom_vec_.push_back(last_odom_vec_[last_odom_vec_.size()-2]);
        current_odom_vec_.push_back(last_odom_vec_[last_odom_vec_.size()-1]);
    }

    pc_frame_.reset(new pcl::PointCloud<myPointType >);
    // cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line
    pcl::fromROSMsg(*laserCloudMsg, *pc_frame_);
    // Remove Nan points
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*pc_frame_, *pc_frame_, indices);

    //findStartEndAngle();
    projectPointCloud();

}

void intervalCal(double time,Eigen::Vector3d& pos,Eigen::Quaterniond& q,bool& is_success_inter){
    bool q_is_finish=false,posF_is_get=false,posB_is_get=false;
    double time_odom_forward,time_odom_back;
    Eigen::Vector3d pos_odom_forward,pos_odom_back;
    for(int i=0;i<last_odom_vec_.size()-1;i++){
        double time_forward=last_odom_vec_[i].header.stamp.toSec();
        double time_back=last_odom_vec_[i+1].header.stamp.toSec();
        if(time_forward<=time&&time<time_back&&!q_is_finish){
            Eigen::Quaterniond q1(last_odom_vec_[i].pose.pose.orientation.w,last_odom_vec_[i].pose.pose.orientation.x,
                                  last_odom_vec_[i].pose.pose.orientation.y,last_odom_vec_[i].pose.pose.orientation.z);
            Eigen::Quaterniond q2(last_odom_vec_[i+1].pose.pose.orientation.w,last_odom_vec_[i+1].pose.pose.orientation.x,
                                  last_odom_vec_[i+1].pose.pose.orientation.y,last_odom_vec_[i+1].pose.pose.orientation.z);
            Eigen::AngleAxisd a_axi(q1.inverse()*q2);
            double interval_angle=a_axi.angle()*(time-time_forward)/(time_back-time_forward);
            Eigen::Quaterniond q_detal(Eigen::AngleAxisd(interval_angle,a_axi.axis()));
            q=q1*q_detal;
            q_is_finish=true;
        }
        if(time_forward<=time&&last_odom_vec_[i].pose.pose.position.x!=odom_x_bad&&!posF_is_get){
            time_odom_forward=time_forward;
            pos_odom_forward=Eigen::Vector3d(last_odom_vec_[i].pose.pose.position.x,last_odom_vec_[i].pose.pose.position.y,
                                             last_odom_vec_[i].pose.pose.position.z);
            posF_is_get=true;
            //ROS_INFO("odom inter forward");
        }
        if(time<time_back&&last_odom_vec_[i+1].pose.pose.position.x!=odom_x_bad&&!posB_is_get){
            time_odom_back=time_back;
            pos_odom_back=Eigen::Vector3d(last_odom_vec_[i+1].pose.pose.position.x,last_odom_vec_[i+1].pose.pose.position.y,
                                             last_odom_vec_[i+1].pose.pose.position.z);
            posB_is_get=true;
            //ROS_INFO("odom inter back");
        }
        if(posF_is_get&&posB_is_get){
            pos=pos_odom_forward+(pos_odom_back-pos_odom_forward)*(time-time_odom_forward)/(time_odom_back-time_odom_forward);
            if(q_is_finish){
                is_success_inter=true;
                return;
            }
        } else if(i==(int)last_odom_vec_.size()-2){
            double time_pos_near=-1;
            for(auto o:last_odom_vec_){
                if(o.pose.pose.position.x!=odom_x_bad){
                    if(time_pos_near==-1){
                        time_pos_near=o.header.stamp.toSec();
                        pos=Eigen::Vector3d(o.pose.pose.position.x,o.pose.pose.position.y,o.pose.pose.position.z);
                        continue;
                    }
                    if(abs(time_pos_near-time)>abs(o.header.stamp.toSec()-time)){
                        time_pos_near=o.header.stamp.toSec();
                        pos=Eigen::Vector3d(o.pose.pose.position.x,o.pose.pose.position.y,o.pose.pose.position.z);
                    }
                }
            }
            //ROS_INFO("pos_cannot_interval");
            is_success_inter= false;
            if(q_is_finish){
                //is_success_inter=true;
                return;
            }
        }
    }
    if(!q_is_finish){
        is_success_inter= false;
        //ROS_INFO("q_cannot_interval");

        double time_pos_near=-1;
        for(auto o:last_odom_vec_){
            if(time_pos_near==-1){
                time_pos_near=o.header.stamp.toSec();
                q=Eigen::Quaterniond(o.pose.pose.orientation.w,o.pose.pose.orientation.x,
                                     o.pose.pose.orientation.y,o.pose.pose.orientation.z);
                continue;
            }
            if(abs(time_pos_near-time)>abs(o.header.stamp.toSec()-time)){
                time_pos_near=o.header.stamp.toSec();
                q=Eigen::Quaterniond(o.pose.pose.orientation.w,o.pose.pose.orientation.x,
                                     o.pose.pose.orientation.y,o.pose.pose.orientation.z);
            }
        }
    }
}

void calibrateMotion(){
    pc_final_.reset(new pcl::PointCloud<myPointType>);
    pc_final_->points.resize(N_SCAN_*Horizon_SCAN_);

    //test
/*    bool test=false,time_inter_test=false;

    for(auto odom:last_odom_vec_){
        ROS_INFO("last odom vec time:%lf",odom.header.stamp.toSec());
        ROS_INFO("last odom vec odom x:%lf",odom.pose.pose.position.x);
        if(odom.pose.pose.position.x!=odom_x_bad)
            ROS_INFO("odom x good");
    }*/
    //ROS_INFO("pc time last:%lf",pc_time_last_);
    //test
    int counter=0;
    int test_counter=1;

    if(pc_time_last_==0){
        pcl::copyPointCloud(*pc_order_,*pc_final_);
    }else{
        double time_scan;
        time_scan=pc_time_last_+time_scan_gap_;
        Eigen::Vector3d pos_base;
        Eigen::Quaterniond q_base;
        bool first_interval_is_success;
        intervalCal(time_scan,pos_base,q_base,first_interval_is_success);



        Eigen::Matrix4d pose_last=Eigen::Matrix4d::Identity(),pose_current=Eigen::Matrix4d::Identity(),pose_acc=Eigen::Matrix4d::Identity();
        pose_last.block<3,3>(0,0)=q_base.matrix();
        pose_last.block<3,1>(0,3)=pos_base;
        for(int i=1;i<Horizon_SCAN_;i++){
            time_scan=time_scan+time_scan_gap_;

            //test
/*
            test_counter++;
            if(test_counter==900){
                ROS_INFO("pc time inter:%lf",time_scan);
                time_inter_test=true;
            }
*/

            //test
            bool is_success_inter;
            Eigen::Vector3d pos_current;
            Eigen::Quaterniond q_current;
            intervalCal(time_scan,pos_current,q_current,is_success_inter);

            if(!is_success_inter){
                counter++;
            }

            pose_current.block<3,3>(0,0)=q_current.matrix();
            pose_current.block<3,1>(0,3)=pos_current;
            pose_acc=pose_acc*pose_last.inverse()*pose_current;
            pose_last=pose_current;
            for(int j=0;j<N_SCAN_;j++){
                if(pc_order_->points[j*Horizon_SCAN_+i].x!=std::numeric_limits<double>::quiet_NaN()){
                    Eigen::Vector4d point_cali(pc_order_->points[j*Horizon_SCAN_+i].x,pc_order_->points[j*Horizon_SCAN_+i].y,pc_order_->points[j*Horizon_SCAN_+i].z,1);
                    point_cali=pose_acc*point_cali;
                    pc_final_->points[j*Horizon_SCAN_+i].x=point_cali(0);
                    pc_final_->points[j*Horizon_SCAN_+i].y=point_cali(1);
                    pc_final_->points[j*Horizon_SCAN_+i].z=point_cali(2);
/*                    if(!test){
                        ROS_INFO("point calibrate pushbcak");
                        test= true;
                    }*/
                }
            }
        }
    }
    pc_time_last_=pc_time_current_;
    ROS_INFO("cannot interval time:%d",counter);
}








void odomCB(const nav_msgs::OdometryConstPtr &msg){
    std::unique_lock<std::mutex> lock(my_mutex_);

    current_odom_.pose.pose.position.x=msg->pose.pose.position.x;
    current_odom_.pose.pose.position.y=msg->pose.pose.position.y;
    current_odom_.pose.pose.position.z=msg->pose.pose.position.z;

    odom_is_far_cur_=Eigen::Vector3d(msg->pose.pose.position.x,msg->pose.pose.position.y,msg->pose.pose.position.z);

    odom_received_ = true;
    //ROS_INFO("odom ok");
}
void imuCB(const sensor_msgs::ImuPtr &msg){
    std::unique_lock<std::mutex> lock(my_mutex_);
    std::unique_lock<std::mutex> lock_odom(odom_vec_mutex_);
    current_odom_.header.stamp=msg->header.stamp;

    Eigen::Quaterniond q_imu(msg->orientation.w,msg->orientation.x,msg->orientation.y,msg->orientation.z);
    Eigen::AngleAxisd q_convert(M_PI,Eigen::Vector3d::UnitZ());
    q_imu=q_convert*q_imu;

    current_odom_.pose.pose.orientation.w=q_imu.w();
    current_odom_.pose.pose.orientation.x=q_imu.x();
    current_odom_.pose.pose.orientation.y=q_imu.y();
    current_odom_.pose.pose.orientation.z=q_imu.z();

    if(odom_received_){
        odom_received_ = false;
        current_odom_vec_.push_back(current_odom_);
        //ROS_INFO("imu odom ok");
        return;
    }else{
        current_odom_.pose.pose.position.x=odom_x_bad;
        current_odom_vec_.push_back(current_odom_);
    }
    //ROS_INFO("imu ok");
}

void displayCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud){
    sensor_msgs::PointCloud2 pc_display;
    //ROS_INFO("test ok");
    pcl::toROSMsg(*cloud,pc_display);
    //ROS_INFO("test ok");
    pc_display.header.frame_id="odom";
    geometry_msgs::TransformStamped trans;
    trans.child_frame_id="base_link";

    trans.transform.rotation.x=pose_show_.pose.pose.orientation.x;trans.transform.rotation.y=pose_show_.pose.pose.orientation.y;
    trans.transform.rotation.z=pose_show_.pose.pose.orientation.z;trans.transform.rotation.w=pose_show_.pose.pose.orientation.w;
    trans.transform.translation.x=pose_show_.pose.pose.position.x;
    trans.transform.translation.y=pose_show_.pose.pose.position.y;
    trans.transform.translation.z=pose_show_.pose.pose.position.z;

    sensor_msgs::PointCloud2 cloud_pub;
    tf2::doTransform(pc_display, cloud_pub, trans);
    cloud_pub.header.frame_id="map";
    cloud_pub.header.stamp=ros::Time::now();;
    cloud_pub_.publish(cloud_pub);
}

void displayOdom(geometry_msgs::Pose pose) {
    geometry_msgs::PoseStamped p;
    p.pose=pose;
    p.header.stamp=ros::Time::now();
    p.header.frame_id="map";
    odom_pose_pub_.publish(p);
    odom_path_.header=p.header;
    odom_path_.poses.push_back(p);
    odom_path_pub_.publish(odom_path_);
    static tf::TransformBroadcaster broadcaster;//1.定义一个广播broadcaster
    tf::Transform transform;//2.声明一个变量用来存储转换信息
    transform.setOrigin(tf::Vector3(pose.position.x,pose.position.y,pose.position.z));
    transform.setRotation(tf::Quaternion(pose.orientation.x,pose.orientation.y,
                                         pose.orientation.z,pose.orientation.w));
    broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "odom"));
}

void cloudCB(const sensor_msgs::PointCloud2ConstPtr &msg){
    //std::unique_lock<std::mutex> lock(my_mutex_);

    getPointCloudAndProject(msg);
    //ROS_INFO("cloud project ok");
    calibrateMotion();
    //ROS_INFO("cloud calibrate ok");

    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*pc_final_, *pc_final_, indices);


    if ((odom_is_far_cur_-odom_is_far_last_).norm()<3&&is_first_get_) {
        return;
    }
    is_first_get_=true;
    odom_is_far_last_=odom_is_far_cur_;


    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud (pc_final_);//这个参数得是指针，类对象不行
    pass.setFilterFieldName ("z");//设置想在哪个坐标轴上操作
    pass.setFilterLimits (-10, 6);//将x轴的0到1范围内
    pass.setFilterLimitsNegative (false);//保留（true就是删除，false就是保留而删除此区间外的）
    pass.filter (*pc_final_);//输出到结果指针

    pass.setInputCloud (pc_final_);//这个参数得是指针，类对象不行
    pass.setFilterFieldName ("x");//设置想在哪个坐标轴上操作
    pass.setFilterLimits (-20, 20);//将x轴的0到1范围内
    pass.setFilterLimitsNegative (false);//保留（true就是删除，false就是保留而删除此区间外的）
    pass.filter (*pc_final_);//输出到结果指针

    pass.setInputCloud (pc_final_);//这个参数得是指针，类对象不行
    pass.setFilterFieldName ("y");//设置想在哪个坐标轴上操作
    pass.setFilterLimits (-20, 20);//将x轴的0到1范围内
    pass.setFilterLimitsNegative (false);//保留（true就是删除，false就是保留而删除此区间外的）
    pass.filter (*pc_final_);//输出到结果指针

    pcl::PCDWriter writer;
    writer.write("/media/st/Application/ubuntu/pointData_calib/pc_frame"
                 +std::to_string(pc_num_)+
                 ".pcd",*pc_final_);
    pc_num_++;




    //publish map
    pose_show_=current_odom_;
    pose_show_.pose.pose.position.x=odom_is_far_last_(0);
    pose_show_.pose.pose.position.y=odom_is_far_last_(1);
    pose_show_.pose.pose.position.z=odom_is_far_last_(2);

    displayCloud(pc_final_);
    displayOdom(pose_show_.pose.pose);
}


int main(int argc,char **argv )
{
    ros::init(argc,argv,"my_pf");
    ros::NodeHandle nh;



    ros::Subscriber odom_sub=nh.subscribe("/odom", 20, odomCB);
    ros::Subscriber imu_sub=nh.subscribe("/imu", 20, imuCB);
    ros::Subscriber cloud_sub=nh.subscribe("/velodyne_points", 10, cloudCB);
    //ros::Subscriber map_sub_ = nh.subscribe("/mapcloud", 1, mapCB);

    //ros::Subscriber mcl_pose_sub=nh.subscribe("/mcl_3dl/pf_base_pose",10,displayMCLBasePose);


    odom_pose_pub_=nh.advertise<geometry_msgs::PoseStamped>("odom_pose",1);
    odom_path_pub_=nh.advertise<nav_msgs::Path>("odom_path",1);
    cloud_pub_=nh.advertise<sensor_msgs::PointCloud2>("cloud_get",1);

    //ros::Rate loop_rate(2000);
    ros::spin();


}
/*

bool isFarEnough(nav_msgs::Odometry current_odom,nav_msgs::Odometry last_odom)
{
    tf::Vector3 distance_=tf::Vector3(current_odom.pose.pose.position.x-last_odom.pose.pose.position.x,
                                      current_odom.pose.pose.position.y-last_odom.pose.pose.position.y,
                                      current_odom.pose.pose.position.z-last_odom.pose.pose.position.z);

*/
/*    tf::Quaternion last_q(last_odom.pose.pose.orientation.x,last_odom.pose.pose.orientation.y,
                          last_odom.pose.pose.orientation.z,last_odom.pose.pose.orientation.w);
    tf::Quaternion current_q(current_odom.pose.pose.orientation.x,current_odom.pose.pose.orientation.y,
                             current_odom.pose.pose.orientation.z,current_odom.pose.pose.orientation.w);*//*


    if(distance_.length()>3){
        return true;
    }

*/
/*    if(abs(current_q.getAngle()-last_q.getAngle())>double(0.15))
    {
        return true;
    }*//*

    return false;
}*/
/*
void displayOdom(geometry_msgs::Pose pose) {
    geometry_msgs::PoseStamped p;
    p.pose=pose;
    p.header.stamp=ros::Time::now();
    p.header.frame_id="map";
    odom_pose_pub_.publish(p);
    odom_path_.header=p.header;
    odom_path_.poses.push_back(p);
    odom_path_pub_.publish(odom_path_);
    static tf::TransformBroadcaster broadcaster;//1.定义一个广播broadcaster
    tf::Transform transform;//2.声明一个变量用来存储转换信息
    transform.setOrigin(tf::Vector3(pose.position.x,pose.position.y,pose.position.z));
    transform.setRotation(tf::Quaternion(pose.orientation.x,pose.orientation.y,
                                         pose.orientation.z,pose.orientation.w));
    broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "odom"));
}

void displayCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud){
    sensor_msgs::PointCloud2 pc_display;
    //ROS_INFO("test ok");
    pcl::toROSMsg(*cloud,pc_display);
    //ROS_INFO("test ok");
    pc_display.header.frame_id="odom";
    geometry_msgs::TransformStamped trans;
    trans.child_frame_id="base_link";

    trans.transform.rotation.x=current_odom_.pose.pose.orientation.x;trans.transform.rotation.y=current_odom_.pose.pose.orientation.y;
    trans.transform.rotation.z=current_odom_.pose.pose.orientation.z;trans.transform.rotation.w=current_odom_.pose.pose.orientation.w;
    trans.transform.translation.x=current_odom_.pose.pose.position.x;
    trans.transform.translation.y=current_odom_.pose.pose.position.y;
    trans.transform.translation.z=current_odom_.pose.pose.position.z;

    sensor_msgs::PointCloud2 cloud_pub;
    tf2::doTransform(pc_display, cloud_pub, trans);
    cloud_pub.header.frame_id="map";
    cloud_pub.header.stamp=ros::Time::now();;
    cloud_pub_.publish(cloud_pub);
}*/
